#include "../../include/robot/GoToPos.h"

using namespace std;
GoToPos::GoToPos(float depth, float degree,VisionController *visione):ArAction("GoToPos", "Va verso quella posizione") {
	this->depth=depth;
	this->degree=degree;
	this->range = 1;
	this->moved = true;
	this->gotopp = false;
	this->visione=visione;
	int cont=0;
}
GoToPos::~GoToPos(){

}
ArActionDesired* GoToPos::fire(ArActionDesired currentDesired){

	myDesired.reset();
	double toRotate = -1;
	if(degree<0)
		toRotate=1;

	if(degree>=-range && degree<=range){
		if(myDesired.getVel()!=400){
			myDesired.setVel(400);
		}
	}else {
		degree= degree+toRotate;
		myDesired.setDeltaHeading(toRotate);
	}

	return &myDesired;
}
